Projectile 3d attitude from 3-axis magnetometer and single-axis accelerometer

ABSTRACT

A method to determine roll angle, pitch angle, and heading angle of a spinning projectile during a flight of the spinning projectile is provided. The method includes providing a magnetic unit vector in an inertial frame of the projectile at a projectile launch location prior to launch of the projectile; determining a magnetic unit vector in a body frame and in an inertial frame of the spinning projectile during the flight of the spinning projectile; determining a velocity unit vector in the body frame and in the inertial frame of the spinning projectile during the flight of the spinning projectile; and calculating the roll angle, the pitch angle, and the heading angle of the spinning projectile during the flight of the spinning projectile, regardless of the spin rate of the projectile. The roll angle and the pitch angle of the spinning projectile form an attitude of the spinning projectile.

BACKGROUND

A projectile fired from a rifled barrel spins too rapidly forinexpensive rate gyroscopes to accurately measure the spin rate,attitude, and heading angle of the projectile. Typically, an inexpensiverate gyroscope is unable to measure spin rates over 100 Hz. Thus,projectiles that incorporate inexpensive rate gyroscopes are typicallydespun after launch in order to reduce the spin rate of the projectileto less than 100 Hz. For example, the spin rate of a despun projectileis slowed down by a protrusion of wings or buffers from the projectilebody. In this manner, the inexpensive rate gyroscopes can measure theattitude and heading angle of the projectile in flight. An exemplaryinexpensive rate gyroscope is a micro-electro-mechanical system (MEMS)rate gyroscope.

SUMMARY

The present application relates to method to determine roll angle, pitchangle, and heading angle of a spinning projectile during a flight of thespinning projectile. The method includes providing a magnetic unitvector in an inertial frame of the projectile at a projectile launchlocation prior to launch of the projectile; determining a magnetic unitvector in a body frame and in an inertial frame of the spinningprojectile during the flight of the spinning projectile; determining avelocity unit vector in the body frame and in the inertial frame of thespinning projectile during the flight of the spinning projectile; andcalculating the roll angle, the pitch angle, and the heading angle ofthe spinning projectile during the flight of the spinning projectile,regardless of the spin rate of the projectile. The roll angle and thepitch angle of the spinning projectile form an attitude of the spinningprojectile.

DRAWINGS

FIG. 1 shows an embodiment of the projectile and three reference framesin accordance with the present invention;

FIGS. 2 and 3 show embodiments of an apparatus used to measure athree-dimensional attitude of a spinning projectile during a flight ofthe spinning projectile in accordance with the present invention;

FIG. 4 shows the plots of the true accelerometer measurements and thelow-pass filtered accelerometer measurements for a 155 mm projectile,spin stabilized about its minor axis with a spin rate of 100 Hz;

FIG. 5 shows an exemplary unit velocity vector on the unit sphere for aprojectile in accordance with the present invention;

FIG. 6 shows a flight path angle for an exemplary projectile inaccordance with the present invention;

FIG. 7 shows the plots of the true flight path angle and estimatedflight path angle for an exemplary 155 mm projectile, spin stabilizedabout its minor axis with a spin rate of 100 Hz;

FIG. 8 shows the plots of the scalar components of the true velocityvector and an estimated velocity vector resolved in the projectile'sNorth East Down frame for a 155 nm projectile, spin stabilized about itsminor axis with a spin rate of 100 Hz;

FIG. 9 is a flow diagram of the vector matching algorithm;

FIG. 10 shows the plots of the direction cosine matrix estimation errorcomputed using the vector matching algorithm;

FIG. 11 shows the plots of the Euler angle estimation errors computedusing the vector matching algorithm; and

FIG. 12 is a flow diagram of one embodiment of a method to measure anddetermine a three-dimensional attitude of a spinning projectile during aflight of the spinning projectile in accordance with the presentinvention.

In accordance with common practice, the various described features arenot drawn to scale but are drawn to emphasize features relevant to thepresent invention. Like reference characters denote like elementsthroughout figures and text.

DETAILED DESCRIPTION

The apparatus described herein is used in a spinning projectile tocalculate a three-dimensional attitude in a spinning projectile, evenwhen the projectile spins with a spin rate greater than 100 Hz.Embodiments of the apparatus include a three-axis magnetometer, asingle-axis accelerometer orientated with the sense axis aligned to thespin axis of the spinning projectile, and at least one algorithmexecutable by a processor to calculate the three-dimensional attitude ofthe projectile during the projectile's traversal from a launch locationto a final destination. In one implementation of this embodiment, the atleast one algorithm includes multiple software modules.

Projectiles without guidance, navigation, and control capability or dumbprojectiles (e.g., dumb munitions) are unable to determine theirthree-dimensional (3D) attitude (i.e., a roll angle, a pitch angle, anda heading angle). The apparatus described herein can be attached to (orinserted into) dumb munitions in order to turn them into smart munitionsthat are able to detect and control their 3D attitude during a flightfrom a launch location to a final destination. Thus, availablestockpiles of dumb munitions can be inexpensively retrofitted as smartmunitions with the apparatus describe herein.

There are several disadvantages of firing projectiles without guidance,navigation, and control capability: 1) the projectile could miss itsintended target; 2) the projectile launch system is exposed to apotential adversary; 3) the cost of each projectile and its launch; and4) the potential collateral damage should the projectile miss itsintended target. Given the existing stockpile of projectiles that arenot equipped with guidance, navigation, and control systems capable ofproviding closed loop control, the advantages of equipping the existingstockpile of projectiles with guidance, navigation, and control systemsare as follows: 1) the current inventory of projectiles can be used inmore demanding applications; 2) the projectiles do not have to bereplaced; and 3) the launch systems do not have to be replaced.

The three-dimensional (3D) attitude of the projectile is stabilized intwo ways. First, the 3D attitude of a projectile is stabilized usingcanards. These projectiles are typically slow spinning (<20 Hz), ordespun, and require in-flight 3D attitude control and in-flighttrajectory guidance. Second, the 3D attitude of the projectile is spinstabilized. These projectiles are fast spinning projectiles. A fastspinning projectile is dynamically stabilized due to its spin rate andis inherently more stable than a despun projectile. Therefore, in idealconditions, fast spinning projectiles require no control effort tostabilize the 3D attitude whereas slow spinning projectiles requirecontrol effort to stabilize the 3D attitude. However, fast spinningprojectiles still require in-flight trajectory guidance becausedisturbance forces including tip-off rates, launch system exit velocityvariations, wind gusts, and other meteorological conditions can causethe projectile to miss its intended target.

The embodiments of apparatuses and methods described herein provide anattitude and heading reference system (AHRS) for accurate, reliable, andlow-cost guidance and navigation that can be attached to existingprojectiles in a low-cost, projectile. For example, the apparatus can beattached to a projectile in a screw-on nose kit. The nose kits includean AHRS that is useful even for fast spinning projectiles, such asgun-launched projectiles with initial angular velocities greater than100 Hz.

FIG. 1 shows an embodiment of the projectile 100 and three referenceframes F_(N), F_(b), F_(s) in accordance with the present invention. Theprojectile 100 includes an apparatus 10 to measure the 3D attitude ofthe projectile 100 after it is launched and as it spins during a flightfrom the launch location 4 to a final destination (not shown). Theapparatus 10 is also referred to herein as “projectile attitude andheading angle reference system 10.” In one implementation of thisembodiment, the apparatus 10 is part of a nose kit that is attached to aprojectile 100. The apparatus 10 is described in detail below withreference to FIG. 2. At the projectile launch location 4, the earth 5has an exemplary local magnetic field the direction of which isindicated by the arrow 50. The magnetic field 50 of the earth 5 does notvary significantly over that distance that a typical projectile 100travels (e.g., tens of kilometers).

FIG. 1 shows the three reference frames used to specify the attitude andheading angle of the projectile 100 traversing the nearly ballistictrajectory 60. The first reference frame is the North East Down (NED)frame, F_(N), which is also referred to herein as the “navigation frameF_(N)” or “inertial frame F_(N)”. The NED frame, F_(N), is spanned bythe vectors (X_(N), Y_(N), Z_(N)). The NED frame, F_(N), has an originlocated at the projectile launch position 4 on the earth 5. The N-axis,or {circumflex over (x)}_(N)-axis, points in the local North direction.The E-axis, or ŷ_(N)-axis, points in the local East direction. The{circumflex over (x)}_(N)-ŷ_(N) plane can be considered a localhorizontal, or local level, plane. The D-axis, or {circumflex over(z)}_(N)-axis, points in the local Down direction. Specifically, the{circumflex over (z)}_(N)-axis is aligned to the earth's gravitationalforce and points down toward the center of the earth 5. Thus, the frameF_(N) is a local vertical local horizontal frame (North, East and Downor NED) that is assumed to be inertial.

The second frame is a projectile body frame, F_(b), which is spanned byvectors (X_(b), Y_(b), Z_(b)). The projectile's body frame, F_(b), isassumed to be the projectile's principal axis frame. The origin of thebody frame F_(b) is located at the center of mass of the projectile 100.The position vector, {right arrow over (r)}, specifies the origin of thebody frame F_(b) relative to the navigation frame F_(N). Without loss ofgenerality, the body frame F_(b) x-axis, or {circumflex over(x)}_(b)-axis, is assumed to point to the front of the projectile 100,the body frame F_(b) y-axis, or ŷ_(b)-axis, coincides with a principalaxis normal to the {circumflex over (x)}_(b)-axis, and the body frameF_(b) z-axis, or {circumflex over (z)}_(b)-axis, coincides with aprincipal axis normal to the {circumflex over (x)}_(b)- and ŷ_(b)-axes.

The third frame is a sensor measurement frame F_(s) which is spanned byvectors (X_(s), Y_(s), Z_(s)). The sensor measurement frame F_(s)specifies the orientation of the sensor measurement axes. The origin ofthe sensor measurement frame F_(s) is located at the sensor position onthe projectile 100. The position vector {right arrow over (r)}_(s)specifies the origin of the sensor measurement frame F_(s) relative tothe body frame F_(b). The sensors locations are fixed on the projectileand, thus, the orientation of the sensor measurement frame F_(s)relative to the body frame F_(B) is fixed and {right arrow over (r)}_(s)is fixed. The principal axis {circumflex over (x)}_(b) of the projectile100 is also the spin axis X_(p) around which the projectile 100 spinsupon launch. The spin axis X_(p) is parallel to the velocity vector{right arrow over (v)} of the spinning projectile 100 during an initialphase of the flight of the spinning projectile 100.

FIGS. 2 and 3 show embodiments of an apparatus used to measure athree-dimensional attitude of a spinning projectile 100 during a flightof the spinning projectile 100 in accordance with the present invention.FIG. 2 shows an embodiment of an apparatus 10 that includes sensors 109,a processor 140, a memory 135, and an attitude estimator module 130 in anon-transitory storage medium 131. The memory 135 stores inertial dataand the magnetic unit vector in the inertial frame of the projectile 100prior to the launch. The magnetic unit vector is provided to theprocessor 140 on the projectile 100 from the three-axis magnetometer110. The processor 140 executes algorithms in the attitude estimatormodule 130. The attitude estimator module 130 includes a low-pass filter129 that is applied to an accelerometer output to estimate drag. Theattitude estimator module 130 also includes a vector matching algorithm133.

The sensors 109 include the three-axis magnetometer 110 and thesingle-axis accelerometer 120 aligned with the projectile's spin axisX_(p) to measure the projectile's attitude (pitch angle and roll angle)and heading angle. In the embodiments described herein, the measurementaxes of the three-axis magnetometer 110 are parallel to the principalaxes of the projectile 100 and the measurement axis 121 of thesingle-axis accelerometer 120 is aligned with the spin axis X_(p) of theprojectile 100. The “single-axis accelerometer 120” is also referred toherein as “accelerometer 120,” and as “spin-axis accelerometer 120.” Thesensors 109 are not subject to frequency jamming (as are GPS sensors),and are not sensitive to environmental conditions of the launch site ortime, such as cloud cover (as are sun sensors and star trackers). In oneimplementation of this embodiment, the low cost sensors aremicro-electro-mechanical system (MEMS) sensors that can withstand 20 KGforces.

The sense axis 121 of the single-axis accelerometer 120 is orientedparallel to the velocity vector {right arrow over (v)} of the spinningprojectile 100 during the flight of the spinning projectile 100.Specifically, prior to launch of the projectile 100, the sense axis 121of the accelerometer 120 is set to be parallel to the {circumflex over(x)}_(b)-axis of the body frame of the projectile 100.

The three-axis magnetometer 100 measures the local magnetic field 50 ofthe earth 5 and outputs information indicative of the local magneticfield 50 to the processor 140 prior to launch of the projectile 100 andduring the flight of the projectile 100. The accelerometer 120 measuresthe velocity of the spinning projectile 100 and outputs informationindicative of the velocity to the processor 140 during the flight of theprojectile 100.

The processor 140 is communicatively coupled to receive an output fromthe sensors 109. The processor 140 is communicatively coupled with thememory 135 and is operable to retrieve the magnetic unit vector in theinertial frame of the projectile 100 that was stored prior to launchduring execution of the algorithms in the attitude estimator module 130.

FIG. 3 shows an embodiment of an apparatus 11 used to measure athree-dimensional attitude of a spinning projectile 100 during a flightof the spinning projectile 100 in accordance with the present invention.The apparatus 11 differs from apparatus 10 in that the non-transitorystorage medium 131 also includes a Kalman filter 132 that iscommunicatively coupled to the attitude estimator module 130. In theapparatus 11, an output from the attitude estimator module 130 isprovided to the Kalman filter 132, which provides additional processing.In one implementation of this embodiment, the Kalman filter 132 computesestimates of the attitude and heading angle of the spinning projectile100.

The operation of the apparatus 10 is now described with reference toFIGS. 1 and 2. The same description is also applicable with reference toFIGS. 1 and 3. The magnetic field 50 is constant in the inertial framealong the trajectory 60 of the projectile 100 and is measured by the3-axis magnetometer 110 in the moving body frame (X_(b), Y_(b), Z_(b))of the projectile 100. The 3-axis magnetometer 110 provides the x, y, zcomponents of the magnetic field 50 in the body frame (X_(b), Y_(b),Z_(b)). The x, y, z components of the magnetic field 50 in the inertialframe (X_(N), Y_(N), Z_(N)) are known from the pre-launch conditions. Inone implementation of this embodiment, the x, y, z components of themagnetic field 50 in the inertial frame (X_(N), Y_(N), Z_(N)) are storedin the memory 135.

The attitude estimator module 130 (FIGS. 2 and 3) includes algorithmsthat: use a low-pass filter 129 on the accelerometer measurements tocompute the drag acting on the projectile 100; estimate the velocityvector of the projectile resolved in the inertial frame 100; estimatethe velocity vector of the projectile resolved in the body frame;generate a 3×3 unit vector matrix; invert the 3×3 unit vector matrix;and multiply the inverted 3×3 unit vector matrix and non-inverted 3×3unit vector matrix to solve for Wahba's problem. Wahba's problem isdescribed in the article entitled “A Least Squares Estimate of SatelliteAttitude” that is published in SIAM Review, Vol. 8, No. 3, (1966)384-386, and which is incorporated herein by reference in its entirety.

The attitude estimator module 130 uses the x, y, z components of themagnetic field 50 in the inertial frame (X_(N), Y_(N), Z_(N)) to the x,y, z components of the magnetic field 50 in the body frame (X_(b),Y_(b), Z_(b)) to generate the direction cosine matrix. However, thisdirection cosine matrix is not unique. The accelerometer 120 providesthe extra measurement needed to determine the direction cosine matrixuniquely. The algorithms (such as vector matching algorithm 133) used bythe attitude estimator module 130 for these functions are shown anddescribed below.

The accelerometer 120 measures the specific force acting along the spinaxis of the projectile 100 and provides information indicative of howmuch the projectile 100 is slowing down due to air drag. During theinitial phase of the propagation, the velocity changes are only due toair drag on the projectile 100 and are not due to nutation of theprojectile 100. Since the velocity vector {right arrow over (v)} and themagnetic field vector are known in both the inertial frame F_(N) and inthe body frame F_(b), the direction cosine matrix between the two framescan be determined uniquely.

The apparatus 10 measures the three-dimensional attitude of theprojectile 100 during the flight. If needed, a correction can besupplied to the projectile 100 to reorient the projectile 100 so thatthe projectile 100 is back on the trajectory required for the projectile100 to reach the final destination. An internal correction signal isgenerated by the processor 140 (FIGS. 2 and 3) to adjust the orientationof the projectile 100 as required to keep the projectile 100 on track tothe final destination. Techniques to reorient the projectile 100 areknown in the art and can be applied after the three-dimensional attitudeof the projectile 100 is known. Since the 3D attitude of the projectile100 is internally adjusted and the location of the projectile 100 is notcommunicatively coupled to any external system, there is no possibilityof jamming the sensor measurements or correction signals to or from theprojectile 100 by a hacker.

A velocity vector and direction cosine matrix are determined for arapidly spinning, nutating, and precessing projectile 100 whose centerof gravity is following the nearly ballistic trajectory 60 using theapparatus 10 mounted on the projectile 100 in the manner describedbelow. This velocity vector and direction cosine matrix are determinedusing sensor data from only the 3-axis magnetometer 110 and thesingle-axis accelerometer 120 with the sense axis 121 (input axis 121)along the spin axis X_(p).

First, the unit vectors of the velocity vector in both the body frameF_(b) and inertial frame F_(N) are determined. The unit vectors of thevelocity vector in both the body frame F_(b) and inertial frame F_(N) isand the known or measured unit vectors of the magnetic field vector 50in both body frame F_(b) and inertial frame F_(N) are used to formulateWahba's problem. Wahba's problem is used to compute a least squaresestimate of the direction cosine matrix between the projectile'snavigation frame F_(N) and body frame F_(b).

Given a set of vectors, u₁ ^(b), u₂ ^(b), . . . , u_(n) ^(b) ∀n≧2,measured in the body frame F_(b) and the same set of vectors known inthe navigation frame F_(N), u₁ ^(N), u₂ ^(N), . . . , u_(n) ^(N) ∀n≧2,the direction cosine matrix that minimizes the performance index iscomputed:

$\begin{matrix}{J_{WP} = {\sum\limits_{i = 2}^{n}{{u_{i}^{b} - {C_{bN}u_{i}^{N}}}}^{2}}} & (1.1)\end{matrix}$

where C_(bN) refers to the direction cosine matrix between F_(N) andF_(b). As applied herein, the superscript following a matrix indicatesthat the scalar components of the corresponding vector are resolvedrelative to the basis vectors of the reference frame indicated by thesuperscript.

In this approach, the direction cosine matrix is computed by matching atleast two vectors that are measured in the body frame F_(b) to the sameset of vectors that are known, or modeled, in the navigation frameF_(N). A unique solution to Wahba's problem requires at least twonon-zero, non-collinear vectors because at least two vectors arerequired to uniquely define a plane. Therefore, if only one vectormeasurement is available, then Wahba's problem does not have a uniquesolution because there is a rotational ambiguity about the axis alignedwith the vector measurement.

Nominally, a spinning projectile in free-fall is subject to onlygravitational force. In this case, the projectile's center of massfollows a parabolic trajectory, and the projectile's spin axis istangent to this parabolic trajectory and is coincident with theprojectile's velocity vector. However, spinning projectiles are subjectto aerodynamic forces such as drag and disturbance forces such astip-off rates, wind gusts, and other meteorological conditions. Theseaerodynamic and disturbance forces cause the projectile's center of massto deviate from the parabolic trajectory and the projectile's spin axisto precess and nutate about the tangent to the projectile's trajectory.The precession and nutation angles are, typically, small. Theprojectile's velocity vector is coincident with the tangent to theprojectile's trajectory.

The attitude and heading angle of the projectile 100 are not observableusing a vector measurement from the three-axis magnetometer 110 at anysingle epoch. However, the measurements of the single-axis accelerometer120 can be manipulated to provide a measurement of the projectile'svelocity vector. If the magnetic field vectors in the navigation frameF_(N) and body frame F_(b) are not parallel to the projectile's velocityvectors in the navigation frame F_(N) and body frame F_(b), then theaccelerometer measurements can resolve the rotational ambiguity inherentin a single three-axis magnetometer measurement. Since the measurementaxis 121 of the accelerometer 120 is aligned with the projectile's spinaxis X_(p), the accelerometer 120 measures components of theprojectile's rotational and linear acceleration.

The vector matching algorithm 133 is used to determine the directioncosine matrix between the projectile's navigation frame and body frameat a single epoch using the following procedure: the local Earthmagnetic field vector is measured using the three-axis magnetometer 110;a vector matching algorithm 133 in the attitude estimator module 130 isprovided with the local Earth magnetic field vector 50 at the launchlocation 4 prior to launch of the projectile 100; the specific force ofthe projectile 100 is measured along its spin axis X_(p); theaccelerometer measurement is filtered to determine the magnitude of theprojectile's various acceleration components; the filtered accelerometermeasurements are used in combination with the magnetometer measurementsto determine the projectile's velocity vector in the projectile'snavigation frame F_(N) and body frame F_(b). As defined herein the“specific force” is the force measured by the accelerometer 120.

The following assumptions regarding the reference frames are made toenable dynamic analysis of the projectile: the NED frame is the inertialframe F_(N); the origin of the NED frame is at the projectile's launchsite; the direction of the NED frame axes are fixed during theprojectile's flight; the projectile's body frame F_(b) is a principalaxis frame.

The following assumptions regarding the sensors 109 are made to enabledynamic analysis of the projectile 100: the sensor locations are fixedon the projectile; the origin of the sensor measurement frame is in the{circumflex over (x)}_(b)-ŷ_(b) plane; the magnetometer measurement axesare parallel to the axes of the projectile's body frame F_(b); themagnetometer 110 is calibrated; the accelerometer measurement axis 121is nominally aligned with the {circumflex over (x)}_(s)-axis.

The following assumptions regarding the projectile dynamics are made toenable dynamic analysis of the projectile: following launch, theprojectile is in a free-fall trajectory subject to drag; the projectileis spin-stabilized along the minor axis with spin rate ≦300 Hz; thelocal Earth magnetic field (EMF) vector does not vary over the lengthand duration of the projectile trajectory and can be determined fromgeodetic survey data, computer models, or direct measurement; thegravity vector is aligned with the {circumflex over (z)}_(N)-axis. Theprocessor 140 enables a low-cost, real-time attitude and heading anglereference system.

Since the vector matching algorithm 133 described herein includes theseabove mentioned assumptions, output from the vector matching algorithm133 based on a simulation of an exemplary fast-spinning projectile inflight is shown in FIGS. 4-8. The simulation provides a check for theperformance of the vector matching algorithm 133 in estimating theattitude and heading angle of the projectile 100.

The vector matching algorithm 133 described in the paper is applicableto fast spinning projectiles. However, in applications withslow-spinning projectiles, the sensor set can be augmented with anorthogonal triad of rate gyros and the vector matching algorithm 133 canbe augmented with a Kalman filter, such as, Kalman filter 132 shown inFIG. 3, to estimate the 3D attitude of the slow-spinning projectile 100.The vector matching algorithm 133 can be augmented, first, by using theattitude estimator module 130 to design the measurement model of theKalman filter 132 and, second, by selecting projectile dynamic modelsfor the Kalman filter 132. These dynamic models can include the 3Dattitude dynamic equations, the attitude kinematic equations, and gyromeasurement models.

The equations of motion (e.g., the kinematic variables, dynamicvariables, equations of motion, and accelerometer measurement equations)for a spinning projectile and the equations relating the accelerometermeasurements to the projectile's kinematic and dynamic variables are nowformulated.

The kinematic variables used to derive the dynamic equations of theprojectile are now defined. The position vector of the projectile'scenter of mass resolved in the projectile's body frame is

r ^(b) =[l _(x) l _(y) l _(z)]^(T)  (2.1)

The position vector of the origin of the sensor measurement framerelative to the origin of the projectile's body frame resolved in theprojectile's body frame is

r _(s) ^(b) =[r _(x) r _(y) r _(z)]^(T)  (2.2)

The angular velocity vector of the projectile, {right arrow over (ω)},resolved in the projectile's body frame is

ω^(b)=[ω_(x)ω_(y)ω_(z)]^(T)  (2.3)

The attitude kinematic equations, parameterized using the directioncosine matrix, is written as

Ċ _(bN)=−(ω^(b))^(x) C _(bN)  (2.4)

where (∘)^(x) refers to the skew-symmetric operator. The velocity vectorof the origin of the sensor measurement frame, {right arrow over(v)}_(s), is

{right arrow over (v)} _(s) ={right arrow over (v)}+{right arrow over(ω)}×{right arrow over (r)} _(s)  (2.5)

where {right arrow over (v)} refers to the velocity vector of theprojectile's center of mass and where

v _(s) ^(b) =[v _(sx) v _(sy) v _(sz)]^(T)

v ^(b) =[v _(x) v _(y) v _(z)]^(T)  (2.6)

The acceleration vector of the origin of the sensor measurement frame,{right arrow over (a)}_(s) is

{right arrow over (a)} _(s) ={right arrow over ({dot over (v)} _(s)|_(b)+{right arrow over (ω)}×{right arrow over (v)} _(s)  (2.7)

Equation (2.5) is substituted into equation (2.7), and {right arrow over(a)}_(s) can be rewritten as

{right arrow over (a)} _(s) ={right arrow over ({dot over (v)}|_(b)+{right arrow over ({dot over (ω)}|_(b) ×{right arrow over (r)} _(s)+{right arrow over (ω)}×{right arrow over (v)}+{right arrow over(ω)}×({right arrow over (ω)}×{right arrow over (r)} _(s)).  (2.8)

The sensor locations are fixed on the projectile. Equation (2.8) isresolved in the projectile's body frame as:

$\begin{matrix}\begin{matrix}{a_{s}^{b} = \begin{bmatrix}a_{sx} & a_{sy} & a_{sz}\end{bmatrix}^{T}} \\{= {{\overset{.}{v}}^{b} + {{\overset{.}{\omega}}^{bx}r_{s}^{b}} + {\omega^{bx}v^{b}} + {\omega^{bx}\omega^{bx}{r_{s}^{b}.}}}}\end{matrix} & (2.9)\end{matrix}$

The accelerometer measurement axis vector, {right arrow over (r)}_(A),resolved in the body frame is

r _(A) ^(b)=[1ε_(y)ε_(z)]^(T)  (2.10)

where ε_(y), ε_(z) refer to the misalignment of the accelerometermeasurement axes from the {circumflex over (x)}_(b)-axis.

The projectile's body frame is set as a principal axis frame, so thesecond (mass) moment of inertia of the projectile, {right arrow over(J)}, is written as

$\begin{matrix}{J_{b} = \begin{bmatrix}J_{x} & 0 & 0 \\0 & J_{y} & 0 \\0 & 0 & J_{z}\end{bmatrix}} & (2.11)\end{matrix}$

Since the projectile is spin stabilized about its minor axis it isassumed that

J _(y) =J _(z) ≧J _(x)  (2.12)

The dynamic equations of the projectile are now derived. Thetranslational equations of motion of the projectile at the projectile'scenter of mass and at the origin of the sensor measurement frame are

$\begin{matrix}{{\frac{\overset{\rightarrow}{F}}{m_{b}} = {{\overset{\rightarrow}{a}}_{s} = {{\overset{.}{\overset{\rightarrow}{v}}{_{b}{+ \overset{.}{\overset{\rightarrow}{\omega}}}}_{b} \times {\overset{\rightarrow}{r}}_{s}} + {\overset{\rightarrow}{\omega} \times \overset{\rightarrow}{v}} + {\overset{\rightarrow}{\omega} \times \left( {\overset{\rightarrow}{\omega} \times {\overset{\rightarrow}{r}}_{s}} \right)}}}}{\frac{\overset{\rightarrow}{F}}{m_{b}} = \left. \overset{.}{\overset{\rightarrow}{v}} \middle| {}_{b}{{+ \overset{\rightarrow}{\omega}} \times \overset{\rightarrow}{v}} \right.}} & (2.13)\end{matrix}$

where {right arrow over (F)} refers to the external forces acting on theprojectile, m_(b) refers to the mass of the projectile, and {right arrowover (a)}_(s) is defined in equation (2.8). The external forces actingon the projectile are

{right arrow over (F)}={right arrow over (F)} _(g) +{right arrow over(F)} _(aero) +{right arrow over (F)} _(M) +{right arrow over (F)}_(d)  (2.14)

where {right arrow over (F)}_(g) refers to the gravity force vector,{right arrow over (F)}_(aero) refers to the aerodynamic drag forcevector, {right arrow over (F)}_(M), refers to the Magnus force vector,and {right arrow over (F)}_(d) refers to force vectors not included inthe first three terms such as disturbance force vectors. Equation (2.14)is resolved in the projectile's body frame as

$\begin{matrix}\begin{matrix}{F^{b} = \begin{bmatrix}F_{x} & F_{y} & F_{z}\end{bmatrix}^{T}} \\{= {F_{g}^{b} + F_{aero}^{b} + F_{M}^{b} + F_{d}^{b}}}\end{matrix} & (2.15)\end{matrix}$

The gravity force vector, resolved in the projectile's body frame, iswritten as

F _(g) ^(b) =C _(bN) F _(g) ^(N) =C _(bN)[0 0g _(E)]^(T)  (2.16)

where

g _(E)=9.81 m/s²  (2.17)

The aerodynamic drag force vector, resolved in the projectile's bodyframe, is written as

F _(aero) ^(b)=−1/2ρS∥{right arrow over (v)}∥(C _(D,xyz) v ^(b) + cC_(ω,xyz)ω^(b))  (2.18)

where ρ refers to the air density, S refers to the cross-sectional areaof the projectile, c projectile chord length (typically the projectile'sdiameter), and C_(D,xyz),C_(ω,xyz) refer to 3×3 matrices of aerodynamicforce coefficients. The matrices of aerodynamic force coefficients arewritten as follows

$\begin{matrix}{{C_{D,{xyz}} = \begin{bmatrix}C_{D\; 0} & 0 & 0 \\0 & C_{L\; \alpha} & 0 \\0 & 0 & C_{L\; \alpha}\end{bmatrix}}{C_{\omega,{xyz}} = 0}} & (2.19)\end{matrix}$

where C_(D0) refers to the projectile's drag coefficient at angle ofattack and C_(Lα) refers to the projectile's side force dragcoefficient. The projectile's side force drag coefficient is set equalto the projectile's normal force coefficient, since it is assumed thatthat the projectile's cross-sectional area is circular. The projectile'sangle of attack is α and the projectile's sideslip angle is β. Theangles α and β are related to the projectile's velocity vector using thefollowing two assumptions 1) ∥{right arrow over (v)}∥≈v_(x) and 2) α andβ are expressed using small angle assumptions. Then α, β and {rightarrow over (v)} are written as

$\begin{matrix}{{\alpha \approx \frac{v_{z}}{v_{x}}}{\beta \approx \frac{v_{y}}{v_{x}}}{\frac{v^{b}}{\overset{\rightarrow}{v}} \approx {\begin{bmatrix}1 & \beta & \alpha\end{bmatrix}^{T}.}}} & (2.20)\end{matrix}$

Equations (2.19) and (2.20) are substituted into equation (2.18), andthe aerodynamic drag force vector is rewritten as

$\begin{matrix}{F_{aero}^{b} = {{- \frac{1}{2}}\rho \; S{{{\overset{\rightarrow}{v}}^{2}\begin{bmatrix}C_{D\; 0} \\{C_{L\; \alpha}\beta} \\{C_{L\; \alpha}\alpha}\end{bmatrix}}.}}} & (2.21)\end{matrix}$

Since α and β are assumed to be small angles α², β²≈0. The gravity forcevector and aerodynamic drag force vector are assumed to be the onlyexternal force vectors acting on the projectile so that

{right arrow over (F)} _(M)=0

{right arrow over (F)} _(d)=0.  (2.22)

The rotational equations of motion of the projectile about its center ofmass are

{right arrow over (G)}={right arrow over (J)}·{right arrow over ({dotover (ω)}| _(b)+{right arrow over (ω)}×({right arrow over (J)}·{rightarrow over (ω)})  (2.23)

where {right arrow over (G)} refers to the external torques acting onthe projectile's center of mass. Equation (2.23) is resolved in theprojectile's body frame, so

{dot over (ω)}^(b) =J _(b) ⁻¹ G ^(b) −J _(b) ⁻¹ω^(bx) J_(b)ω^(b)  (2.24)

The external torques acting on the projectile's center of mass are

{right arrow over (G)}={right arrow over (r)} _(p) ×{right arrow over(F)} _(aero) +{right arrow over (r)} _(M) ×{right arrow over (F)} _(M)+{right arrow over (G)} _(aero) +{right arrow over (G)} _(d)  (2.25)

where {right arrow over (r)}_(p) refers to the position vector of theprojectile's center of pressure relative to the projectile's center ofmass, {right arrow over (r)}_(M) refers to the position vector of theprojectile's center of Magnus force relative to the projectile's centerof mass, {right arrow over (G)}_(aero) refers to the aerodynamic torquevector, and refers to torque vectors not included in the first threeterms such as disturbance torque vectors. Equation (2.25) is resolved inthe projectile's body frame, so

$\begin{matrix}\begin{matrix}{G^{b} = \begin{bmatrix}G_{x} & G_{y} & G_{z}\end{bmatrix}^{T}} \\{= {{r_{p}^{b \times}F_{aero}^{b}} + {r_{M}^{b \times}F_{M}^{b}} + G_{aero}^{b} + {G_{d}^{b}.}}}\end{matrix} & (2.26)\end{matrix}$

The projectile's center of pressure is assumed to lie along theprojectile's {circumflex over (x)}_(b)-axis so that

r _(p) ^(b) =[r _(px)0 0]^(T).  (2.27)

The torque acting on the projectile's center of mass due to theaerodynamic drag force vector, resolved in the projectile's body frame,is written by substituting equations (2.21) and (2.27) into equation(2.26)

$\begin{matrix}\begin{matrix}{{r_{p}^{b \times}F_{aero}^{b}} = {{- \frac{1}{2}}\rho \; S{{{\overset{\rightarrow}{v}}^{2}\begin{bmatrix}r_{px} \\0 \\0\end{bmatrix}}^{\times}\begin{bmatrix}C_{D\; 0} \\{C_{L\; \alpha}\beta} \\{C_{L\; \alpha}\alpha}\end{bmatrix}}}} \\{= {{- \frac{1}{2}}\rho \; S{{\overset{\rightarrow}{v}}^{2}\begin{bmatrix}0 \\{{- r_{px}}C_{L\; \alpha}\alpha} \\{r_{px}C_{L\; \alpha}\beta}\end{bmatrix}}}}\end{matrix} & (2.28)\end{matrix}$

The torque acting on the projectile's center of mass due to the gravityforce vector is assumed to be the only external torque vector acting onthe projectile's center of mass so that

{right arrow over (r)} _(M) ×{right arrow over (F)} _(M)={right arrowover (0)}

{right arrow over (G)} _(aero)={right arrow over (0)}

{right arrow over (G)} _(d)={right arrow over (0)}  (2.29)

The equations relating the accelerometer measurements to theprojectile's kinematic and dynamic variables are now derived.Specifically, the projectile's acceleration and external forcecomponents measured by the single axis accelerometer aligned with thesensor's {circumflex over (x)}_(s)-axis are derived. In general,

$\begin{matrix}{{\overset{\rightarrow}{f}}_{s} = {{\overset{\rightarrow}{a}}_{s} - \frac{{\overset{\rightarrow}{F}}_{g}}{m_{b}}}} & (2.30)\end{matrix}$

where {right arrow over (f)}_(s) refers to the accelerometermeasurements. Equation (2.8) is substituted into equation (2.30), and{right arrow over (f)}_(s) is rewritten as

$\begin{matrix}{{\overset{\rightarrow}{f}}_{s} = {{\overset{.}{\overset{\rightarrow}{v}}{_{b}{+ \overset{.}{\overset{\rightarrow}{\omega}}}}_{b} \times {\overset{\rightarrow}{r}}_{s}} + {\overset{\rightarrow}{\omega} \times \overset{\rightarrow}{v}} + {\overset{\rightarrow}{\omega} \times \left( {\overset{\rightarrow}{\omega} \times {\overset{\rightarrow}{r}}_{s}} \right)} - {\frac{{\overset{\rightarrow}{F}}_{g}}{m_{b}}.}}} & (2.31)\end{matrix}$

By appealing to the vector identity

{right arrow over (u)}×({right arrow over (v)}×{right arrow over(w)})=({right arrow over (u)}·{right arrow over (w)}){right arrow over(v)}−({right arrow over (w)}{right arrow over (u)}){right arrow over(v)},  (2.32)

equation (2.31) is rewritten as

$\begin{matrix}{{\overset{\rightarrow}{f}}_{s} = {{\overset{.}{\overset{\rightarrow}{v}}{_{b}{+ \overset{.}{\overset{\rightarrow}{\omega}}}}_{b} \times {\overset{\rightarrow}{r}}_{s}} + {\overset{\rightarrow}{\omega} \times \overset{\rightarrow}{v}} - {\left( {{{\overset{\rightarrow}{\omega} \cdot \overset{\rightarrow}{\omega}}\overset{\rightarrow}{I}} - {\overset{\rightarrow}{\omega}\mspace{11mu} \overset{\rightarrow}{\omega}}} \right){\overset{\rightarrow}{r}}_{s}} - \frac{{\overset{\rightarrow}{F}}_{g}}{m_{b}}}} & (2.33) \\{{\overset{\rightarrow}{f}}_{s} = {{\overset{.}{\overset{\rightarrow}{v}}{_{b}{+ \overset{.}{\overset{\rightarrow}{\omega}}}}_{b} \times {\overset{\rightarrow}{r}}_{s}} + {\overset{\rightarrow}{\omega} \times \overset{\rightarrow}{v}} + {\overset{\rightarrow}{\omega} \times \left( {\overset{\rightarrow}{\omega} \times {\overset{\rightarrow}{r}}_{s}} \right)} - {\frac{{\overset{\rightarrow}{F}}_{g}}{m_{b}}.}}} & (2.31)\end{matrix}$

By appealing to the vector identity

{right arrow over (u)}×({right arrow over (v)}×{right arrow over(w)})=({right arrow over (u)}·{right arrow over (w)}){right arrow over(v)}−({right arrow over (w)}{right arrow over (u)}){right arrow over(v)},  (2.32)

equation (2.31) is rewritten as

$\begin{matrix}{{\overset{\rightarrow}{f}}_{s} = {{\overset{.}{\overset{\rightarrow}{v}}{_{b}{+ \overset{.}{\overset{\rightarrow}{\omega}}}}_{b} \times {\overset{\rightarrow}{r}}_{s}} + {\overset{\rightarrow}{\omega} \times \overset{\rightarrow}{v}} - {\left( {{{\overset{\rightarrow}{\omega} \cdot \overset{\rightarrow}{\omega}}\overset{\rightarrow}{I}} - {\overset{\rightarrow}{\omega}\mspace{11mu} \overset{\rightarrow}{\omega}}} \right){\overset{\rightarrow}{r}}_{s}} - {\frac{{\overset{\rightarrow}{F}}_{g}}{m_{b}}.}}} & (2.33)\end{matrix}$

Equation (2.33) indicates that {right arrow over (f)}_(s) is a functionof the body derivatives of the velocity vector and angular velocityvector. Substituting these derivative vector terms using the kinematicand dynamic equations developed above, {right arrow over (f)}_(s) iswritten as a function of the velocity vector and the angular velocityvector. Therefore, the accelerometer measurements are functions of theprojectile's velocity and angular velocity.

By substituting equations (2.7), (2.13), (2.14), and (2.22) intoequation (2.33), then {right arrow over (f)}_(s) is rewritten as

$\begin{matrix}\begin{matrix}{{\overset{\rightarrow}{f}}_{s} = \left. {\left( {\frac{{\overset{\rightarrow}{F}}_{g} + {\overset{\rightarrow}{F}}_{aero} + {\overset{\rightarrow}{F}}_{M} + {\overset{\rightarrow}{F}}_{d}}{m_{b}} - {\overset{\rightarrow}{\omega} \times \overset{\rightarrow}{v}}} \right) + \overset{.}{\overset{\rightarrow}{\omega}}} \middle| {}_{b} \times \right.} \\{{{\overset{\rightarrow}{r}}_{s} + {\overset{\rightarrow}{\omega} \times \overset{\rightarrow}{v}} - {\left( {{{\overset{\rightarrow}{\omega} \cdot \overset{\rightarrow}{\omega}}\overset{\rightarrow}{I}} - {\overset{\rightarrow}{\omega}\mspace{11mu} \overset{\rightarrow}{\omega}}} \right){\overset{\rightarrow}{r}}_{s}} - \frac{{\overset{\rightarrow}{F}}_{g}}{m_{b}}}} \\{= \left. {\frac{{\overset{\rightarrow}{F}}_{aero}}{m_{b}} + \overset{.}{\overset{\rightarrow}{\omega}}} \middle| {}_{b}{{\times {\overset{\rightarrow}{r}}_{s}} - {\left( {{{\overset{\rightarrow}{\omega} \cdot \overset{\rightarrow}{\omega}}\overset{\rightarrow}{I}} - {\overset{\rightarrow}{\omega}\mspace{11mu} \overset{\rightarrow}{\omega}}} \right){{\overset{\rightarrow}{r}}_{s}.}}} \right.}\end{matrix} & (2.34)\end{matrix}$

Equation (2.34) indicates that the projectile's velocity vector affectsthe accelerometer measurements through the aerodynamic drag forcevector. Equation (2.34) is resolved in the projectile's body frame, sothat:

$\begin{matrix}{f_{s}^{b} = {\frac{F_{aero}^{b}}{m_{b}} + {{\overset{.}{\omega}}_{b}^{b \times}r_{s}^{b}} - {\left( {{\omega_{b}^{bT}\omega_{b}^{b}I} - {\omega_{b}^{b}\omega_{b}^{bT}}} \right){r_{s}^{b}.}}}} & (2.35)\end{matrix}$

Substituting equation (2.24) into equation (2.35), allows {right arrowover (f)}_(s) to be rewritten as:

$\begin{matrix}\begin{matrix}{f_{s}^{b} = {\frac{F_{aero}^{b}}{m_{b}} + {\left( {{J_{b}^{- 1}G^{b}} - {J_{b}^{- 1}\omega^{b \times}J_{b}\omega^{b}}} \right)^{\times}r_{s}^{b}} -}} \\{{\left( {{\omega_{b}^{bT}\omega_{b}^{b}I} - {\omega_{b}^{b}\omega_{b}^{bT}}} \right)r_{s}^{b}}} \\{= {\frac{F_{aero}^{b}}{m_{b}} + {\begin{pmatrix}{\left( {{J_{b}^{- 1}G^{b}} - {J_{b}^{- 1}\omega^{b \times}J_{b}\omega^{b}}} \right)^{\times} -} \\{{\omega_{b}^{bT}\omega_{b}^{b}I} + {\omega_{b}^{b}\omega_{b}^{bT}}}\end{pmatrix}{r_{s}^{b}.}}}}\end{matrix} & (2.36)\end{matrix}$

The single axis accelerometer is aligned with the projectile's{circumflex over (x)}_(b)-axis. However, accelerometer misalignment withthe projectile's {circumflex over (x)}_(b)-axis introduces theprojectile's spin rate components into the accelerometer measurements,equation (2.10). In its scalar component form as resolved in F_(b), thesingle axis accelerometer aligned with the projectile's {circumflex over(x)}_(b)-axis measures the following aerodynamic drag force and angularvelocity components.

$\begin{matrix}\begin{matrix}{f_{s}^{b} = {\frac{\rho \; S{\overset{\rightarrow}{v}}^{2}C_{D\; 0}}{2m_{b}} + {{\begin{bmatrix}1 \\ɛ_{y} \\ɛ_{z}\end{bmatrix}^{T}\begin{bmatrix}{J_{x}^{- 1}\left( {G_{x} - {\omega_{y}{\omega_{z}\left( {J_{z} - J_{y}} \right)}}} \right)} \\{J_{y}^{- 1}\left( {G_{y} - {\omega_{x}{\omega_{z}\left( {J_{x} - J_{z}} \right)}}} \right)} \\{J_{z}^{- 1}\left( {G_{z} - {\omega_{x}{\omega_{y}\left( {J_{y} - J_{x}} \right)}}} \right)}\end{bmatrix}}^{\times}\begin{bmatrix}r_{x} \\r_{y} \\r_{z}\end{bmatrix}} +}} \\{{{\begin{bmatrix}1 \\ɛ_{y} \\ɛ_{z}\end{bmatrix}^{T}\begin{bmatrix}{{- \omega_{y}^{2}} - \omega_{z}^{2}} & {\omega_{x}\omega_{y}} & {\omega_{x}\omega_{z}} \\{\omega_{x}\omega_{y}} & {{- \omega_{x}^{2}} - \omega_{z}^{2}} & {\omega_{y}\omega_{z}} \\{\omega_{x}\omega_{z}} & {\omega_{y}\omega_{z}} & {{- \omega_{x}^{2}} - \omega_{y}^{2}}\end{bmatrix}}\begin{bmatrix}r_{x} \\r_{y} \\r_{z}\end{bmatrix}}} \\{= {\frac{\rho \; S{\overset{\rightarrow}{v}}^{2}C_{D\; 0}}{2m_{b}} + \begin{bmatrix}1 \\ɛ_{y} \\ɛ_{z}\end{bmatrix}^{T}}} \\{{\begin{bmatrix}{{{- {J_{z}^{- 1}\left( {G_{z} - {\omega_{x}{\omega_{y}\left( {J_{y} - J_{x}} \right)}}} \right)}}r_{y}} + {{J_{y}^{- 1}\left( {G_{y\;} - {\omega_{x}{\omega_{z}\left( {J_{x} - J_{z}} \right)}}} \right)}r_{z}}} \\{{{J_{z}^{- 1}\left( {G_{z} - {\omega_{x}{\omega_{y}\left( {J_{y} - J_{x}} \right)}}} \right)}r_{x}} - {{J_{x}^{- 1}\left( {G_{x\;} - {\omega_{y}{\omega_{z}\left( {J_{z} - J_{y}} \right)}}} \right)}r_{z}}} \\{{{- {J_{y}^{- 1}\left( {G_{y} - {\omega_{x}{\omega_{z}\left( {J_{x} - J_{z}} \right)}}} \right)}}r_{x}} + {{J_{x}^{- 1}\left( {G_{x\;} - {\omega_{y}{\omega_{z}\left( {J_{z} - J_{y}} \right)}}} \right)}r_{y}}}\end{bmatrix} +}} \\{{\begin{bmatrix}1 \\ɛ_{y} \\ɛ_{z}\end{bmatrix}^{T}\begin{bmatrix}{{\left( {{- \omega_{y}^{2}} - \omega_{z}^{2}} \right)r_{x}} + {\omega_{x}\omega_{y}r_{y}} + {\omega_{x}\omega_{z}r_{z}}} \\{{\omega_{x}\omega_{y}r_{x}} + {\left( {{- \omega_{x}^{2}} - \omega_{z}^{2}} \right)r_{y}} + {\omega_{y}\omega_{z}r_{z}}} \\{{\omega_{x}\omega_{z}r_{x}} + {\omega_{y}\omega_{z}r_{y}} + {\left( {{- \omega_{x}^{2}} - \omega_{y}^{2}} \right)r_{z}}}\end{bmatrix}}}\end{matrix} & (2.38) \\{f_{sx}^{b} = {{- \frac{\rho \; S{\overset{\rightarrow}{v}}^{2}C_{D\; 0}}{2m_{b}}} - {{J_{z}^{- 1}\left( {G_{z} - {\omega_{x}{\omega_{y}\left( {J_{y} - J_{x}} \right)}}} \right)}r_{y}} + {{J_{y}^{- 1}\left( {G_{y} - {\omega_{x}{\omega_{z}\left( {J_{x} - J_{z}} \right)}}} \right)}r_{z}} + {ɛ_{y}\left( {{{J_{z}^{- 1}\left( {G_{z} - {\omega_{x}{\omega_{y}\left( {J_{y} - J_{x}} \right)}}} \right)}r_{x}} - {{J_{x}^{- 1}\left( {G_{x} - {\omega_{y}{\omega_{z}\left( {J_{z} - J_{y}} \right)}}} \right)}r_{z}}} \right)} + {ɛ_{z}\left( {{{- {J_{y}^{- 1}\left( {G_{y\;} - {\omega_{x}{\omega_{z}\left( {J_{x} - J_{z}} \right)}}} \right)}}r_{x}} + {{J_{x}^{- 1}\left( {G_{x} - {\omega_{y}{\omega_{z}\left( {J_{z} - J_{y}} \right)}}} \right)}r_{y\;}}} \right)} + {\left( {{- \omega_{y}^{2}} - \omega_{z}^{2}} \right)r_{x}} + {\omega_{x}\omega_{y\;}r_{y}} + {\omega_{x}\omega_{z}r_{z}} + {ɛ_{y}\left( {{\omega_{x}\omega_{y}r_{x}} + {\left( {{- \omega_{x}^{2}} - \omega_{y}^{2}} \right)r_{y}} + {\omega_{y}\omega_{z}r_{z}}} \right)} + {ɛ_{z}\left( {{\omega_{x}\omega_{z}r_{x}} + {\omega_{y}\omega_{z}r_{y}} + {\left( {{- \omega_{x}^{2}} - \omega_{y}^{2}} \right)r_{z}}} \right)}}} & \;\end{matrix}$

Equations (2.12) and (2.28) are substituted into equation (2.38), so

$\begin{matrix}{f_{sx}^{b} \approx {{- \frac{\rho \; S{\overset{\rightarrow}{v}}^{2}C_{D\; 0}}{2m_{b}}} + {\frac{\rho \; S{\overset{\rightarrow}{v}}^{2}r_{px}C_{L\; \alpha}\beta}{2J_{z}}r_{y}} + {\frac{\omega_{x}{\omega_{y}\left( {J_{y} - J_{x}} \right)}}{J_{z}}r_{y}} + {\frac{\rho \; S{\overset{\rightarrow}{v}}^{2}r_{px}C_{L\; \alpha}\alpha}{2J_{y}}r_{z}} - {\frac{\omega_{x}{\omega_{z}\left( {J_{x} - J_{z}} \right)}}{J_{y}}r_{z}} + {\left( {{- \omega_{y}^{2}} - \omega_{z}^{2}} \right)r_{x}} + {\omega_{x}\omega_{y}r_{y}} + {\omega_{x}\omega_{z}r_{z}} + {ɛ_{y}\left( {{\frac{G_{z\;}}{J_{z}}r_{x}} - {\frac{\omega_{x}{\omega_{y}\left( {J_{y} - J_{x}} \right)}}{J_{z}}r_{x}} + {\omega_{x}\omega_{y}r_{x}} + {\left( {{- \omega_{x}^{2}} - \omega_{z}^{2}} \right)r_{y}} + {\omega_{y}\omega_{z}r_{z}}} \right)} + {{ɛ_{z}\left( {{\frac{G_{y\;}}{J_{y}}r_{x}} + {\frac{\omega_{x}{\omega_{z}\left( {J_{x} - J_{z}} \right)}}{J_{y}}r_{x}} + {\omega_{x}\omega_{z}r_{x}} + {\omega_{y}\omega_{z}r_{y}} + {\left( {{- \omega_{x}^{2}} - \omega_{y}^{2}} \right)r_{z}}} \right)}.}}} & (2.39)\end{matrix}$

The projectile is assumed to be spin stabilized about its minor axis soequation (2.12) is satisfied, and

$\begin{matrix}{{{\omega_{x}^{2}r_{y}}\operatorname{>>}{{\frac{G_{z}}{J_{z}}r_{x}} - {\frac{\omega_{x}{\omega_{y}\left( {J_{y} - J_{x}} \right)}}{J_{z}}r_{x}} + {\omega_{x}\omega_{y}r_{x}} - {\omega_{z}^{2}r_{y}} + {\omega_{y}\omega_{z}r_{z}}}}{{\omega_{x}^{2}r_{z}}\operatorname{>>}{{{{- \frac{G_{y}}{J_{y}}}r_{x}} + {\frac{\omega_{x}{\omega_{z}\left( {J_{x} - J_{z}} \right)}}{J_{y}}r_{x}} + {\omega_{x}\omega_{z}r_{x}} + {\omega_{y}\omega_{z}r_{y}} + {\omega_{y}^{2}r_{z}\frac{J_{y} - J_{x}}{J_{z}}}} = {\frac{J_{z} - J_{x}}{J_{y}} \approx 1.}}}} & (2.40)\end{matrix}$

Equation (2.40) is substituted into equation (2.39), so

$\begin{matrix}{f_{sx}^{b} \approx {{- \frac{\rho \; S{\overset{->}{v}}^{2}C_{D\; 0}}{2m_{b}}} + {\frac{\rho \; S{\overset{->}{v}}^{2}r_{px}C_{L\; \alpha}}{2J_{z}}\left( {{\beta \; r_{y}} + {\alpha \; r_{z}}} \right)} - {\left( {\omega_{y}^{2} + \omega_{z}^{2}} \right)r_{x}} + {2{\omega_{x}\left( {{\omega_{y}r_{y}} + {\omega_{z}r_{z}}} \right)}} - {{\omega_{x}^{2}\left( {{ɛ_{y}r_{y}} + {ɛ_{z}r_{z}}} \right)}.}}} & (2.41)\end{matrix}$

Equation (2.41) indicates that the accelerometer measurements includeboth low-frequency components and high-frequency components. Theaccelerometer measurements are low- and high-pass filtered, so

$\begin{matrix}{{\text{low-pass}\left( f_{sx}^{b} \right)} \approx {{- \frac{\rho \; S{\overset{->}{v}}^{2}C_{D\; 0}}{2m_{b}}} - {\left( {\omega_{y}^{2} + \omega_{z}^{2}} \right)r_{x}} - {\omega_{x}^{2}\left( {{ɛ_{y}r_{y}} + {ɛ_{z}r_{z}}} \right)}}} & (2.42) \\{{\text{high-pass}\left( f_{sx}^{b} \right)} \approx {{\frac{\rho \; S{\overset{->}{v}}^{2}r_{px}C_{L\; \alpha}}{2J_{z}}\left( {{\beta \; r_{y}} + {\alpha \; r_{z}}} \right)} + {2\; {{\omega_{x}\left( {{\omega_{y}r_{y}} + {\omega_{z}r_{z}}} \right)}.}}}} & \;\end{matrix}$

The side force drag coefficient is assumed to be 0, so

C _(Lα)=0.  (2.43)

Then equation (2.42) is rewritten as

$\begin{matrix}{\text{low-pass}\begin{matrix}{\left( f_{sx}^{b} \right) \approx {{- \frac{\rho \; S{\overset{->}{v}}^{2}C_{D\; 0}}{2m_{b}}} - {\left( {\omega_{y}^{2} + \omega_{z}^{2}} \right)r_{x}} - {\omega_{x}^{2}\left( {{ɛ_{y}r_{y}} + {ɛ_{z}r_{z}}} \right)}}} \\{\approx {{- \frac{\rho \; S{\overset{->}{v}}^{2}C_{D\; 0}}{2m_{b}}} - {\left( {\omega_{y}^{2} + \omega_{z}^{2}} \right)r_{x}}}}\end{matrix}} & (2.44) \\{ {{\text{high-pass}\left( f_{sx}^{b} \right)} \approx {2{{\omega_{x}\left( {{\omega_{y}r_{y}} + {\omega_{z}r_{z}}} \right)}.}}}} & \;\end{matrix}$

The term ω_(x) ²(ε_(y)r_(y)+ε_(z)r_(z)) is an error component due toaccelerometer mounting misalignment within the projectile.

Since a number of assumptions and simplifications were used to deriveequations (2.41) and (2.44), a verification process is described. Thetrue accelerometer measurements and the filtered accelerometercomponents are plotted to verify that the filtered components capturethe dynamics of the true accelerometer measurements. FIG. 4 shows theplots of the true accelerometer measurements 340 and the low-passfiltered accelerometer measurements 350 (Equation 2.44) for a 155 mmprojectile, spin stabilized about its minor axis with a spin rate of 100Hz. As is shown in FIG. 4, the low-pass filtered component of theaccelerometer measurements 350 are dominated by the aerodynamic dragforce.

The vector matching algorithm 133 for the attitude estimator module 130in the apparatus 10 or 11 is now derived. As described above, the firstobjective of this report is to design an attitude and heading anglereference system for a fast spinning projectile using Wahba's problem,or vector matching. This objective can be restated as computing theattitude and heading angle of a fast spinning projectile using vectormatching. This computation is done based on the measurements from thethree-axis magnetometer 110 and the single axis accelerometer 120aligned with the projectile's spin axis X_(p). The solution of Wahba'sproblem, equation (1.1), is

$\begin{matrix}{{C_{bN}\begin{bmatrix}\frac{v^{N}}{v^{N}} & \frac{m^{N}}{m^{N}} & {\frac{v^{N \times}}{v^{N}}\frac{m^{N}}{m^{N}}}\end{bmatrix}} = \begin{bmatrix}\frac{v^{b}}{v^{b}} & \frac{m^{b}}{m^{b}} & {\frac{v^{b \times}}{v^{b}}\frac{m^{b}}{m^{b}}}\end{bmatrix}} & (3.1) \\{C_{bN} = {\begin{bmatrix}\frac{v^{b}}{v^{b}} & \frac{m^{b}}{m^{b}} & {\frac{v^{b \times}}{v^{b}}\frac{m^{b}}{m^{b}}}\end{bmatrix}\begin{bmatrix}\frac{v^{N}}{v^{N}} & \frac{m^{N}}{m^{N}} & {\frac{v^{N \times}}{v^{N}}\frac{m^{N}}{m^{N}}}\end{bmatrix}}^{- 1}} & \; \\{\left\lbrack {U,\Sigma,V} \right\rbrack = {{svd}\left( C_{bN} \right)}} & \; \\{C_{bN\_ final} = {{UV}^{- 1}.}} & \;\end{matrix}$

The term svd shown in equation 3.1 is referred to herein as a singularvalue decomposition that is used to calculate generate a directioncosine matrix C_(bN) _(—) _(final) from the direction cosine matrixC_(bN) calculated to solve Wahba's problem in order to enforceorthogonality constraints on the direction cosine matrix C_(bN)calculated to solve Wahba's problem. The projectile's attitude andheading angle are computed from C_(bN) _(—) _(final). Equation (3.1)requires the following information to compute C_(bN) _(—) _(final): 1)the magnitude and direction of the local Earth magnetic field vector inthe projectile's body frame; 2) the magnitude and direction of the localEarth magnetic field vector in the NED frame; 3) the magnitude anddirection of the projectile's velocity vector in the projectile's bodyframe; and 4) the magnitude and direction of the projectile's velocityvector in the NED frame.

The methodology required to use the local Earth magnetic field vector inthe vector matching algorithm 133 is now described. The vector matchingalgorithm 133 requires the components of the local Earth magnetic fieldvector resolved in both the projectile's body frame and the NED frame.The measurements from the three-axis magnetometer 110 provide thecomponents of the local Earth magnetic field vector resolved in theprojectile's body frame because the axes of the sensor measurement frameare aligned with the axes of the projectile's body frame. Themagnetometer measurements are assumed to be calibrated.

The vector matching algorithm 133 requires the magnitude and directionof the projectile's velocity vector in the NED frame and theprojectile's body frame. The methodology required to compute themagnitude of the projectile's velocity vector, the direction of theprojectile's velocity vector in the NED frame, and the direction of theprojectile's velocity vector in the projectile's body frame is nowdescribed.

The magnitude of the projectile's velocity vector is determined from theaccelerometer measurement equation, equation (2.41). FIG. 4 shows thatthe low-pass filtered accelerometer measurements are governed by theaerodynamic drag force vector. Therefore, the low-pass filteredaccelerometer measurements are rewritten as

$\begin{matrix}{{\text{low-pass}\left( {f_{sx}^{b}(t)} \right)} \approx {- \frac{\rho \; S{{\overset{->}{v}(t)}}^{2}C_{D\; 0}}{2m_{b}}}} & (3.2)\end{matrix}$

Equation (3.2) is rearranged as:

$\begin{matrix}{{{\overset{\hat{->}}{v}(t)}} = \sqrt{- \frac{2m_{b}\text{low-pass}\left( {f_{sx}^{b}(t)} \right)}{\rho \; {SC}_{D\; 0}}}} & (3.3)\end{matrix}$

where ∥{right arrow over ({circumflex over (v)}∥ refers to the magnitudeof the projectile's velocity vector computed by low-pass filtering theaccelerometer measurements. Equation (3.3) indicates that theaerodynamic drag force vector is the key component of the accelerometermeasurements used to estimate the magnitude of the projectile's velocityvector.

The direction of the projectile's velocity vector in the projectile'sNED frame is now determined. FIG. 5 shows an exemplary unit velocityvector on the unit sphere 200 for a projectile 100 in accordance withthe present invention. The tip of the unit velocity vector v^(N)/∥v^(N)∥is constrained to lie on the surface of an exemplary unit sphere 200 sothat:

$\begin{matrix}{{\frac{v^{N}}{v^{N}} \cdot \frac{v^{N}}{v^{N}}} = 1.} & (3.4)\end{matrix}$

FIG. 6 shows a flight path angle for an exemplary projectile inaccordance with the present invention. The tip of the unit vectorv^(N)/∥v^(N)∥ is constrained to lie in the plane defined by theprojectile's flight path angle, γ.

$\begin{matrix}\begin{matrix}{{\frac{g^{N}}{g^{N}} \cdot \left( {- \frac{v^{N}}{v^{N}}} \right)} = {\cos \left( {{90{^\circ}} - \gamma} \right)}} \\{= {\sin \; \gamma}} \\{{\frac{g^{N}}{g^{N}} \cdot \frac{v^{N}}{v^{N}}} = {{- \sin}\; {\gamma.}}}\end{matrix} & (3.5)\end{matrix}$

The plane defined by γ and the unit sphere 200 intersect in a circle.Therefore, the projectile's flight path angle constrains the tip of theunit vector v^(N)/∥v^(N)∥ to lie on the intersection of the plane atvertical distance sin γ from the center C_(sphere) of the unit sphere200 and the sphere 200. The projectile's flight path angle γ is computedfrom the accelerometer measurements. The governing equation for flightpath angle is

$\begin{matrix}{{{\overset{\overset{.}{\hat{}}}{\gamma}(t)} = {- \frac{g_{E}\cos \; {\hat{\gamma}(t)}}{{\overset{\hat{->}}{v}(t)}}}}{{\gamma (0)} \equiv \text{projectile launch angle.}}} & (3.6)\end{matrix}$

Equation 2.17 defines g_(E). ∥{right arrow over ({circumflex over (v)}∥is determined from the low-pass filtered accelerometer measurements, andγ(0) is determined from accelerometer measurements just prior toprojectile launch.

Since a number of assumptions and simplifications were made in order toderive ∥{right arrow over ({circumflex over (v)}∥ in equation (3.6) theprojectile's true flight path angle and estimated flight path angle areplotted to verify that the projectile's estimated speed captures thedynamics of the projectile's true speed. FIG. 7 shows the plots of thetrue flight path angle and estimated flight path angle for a 155 mmprojectile, spin stabilized about its minor axis with a spin rate of 100Hz. A 4^(th)-order low-pass filter was used to estimate the projectile'sspeed from the aerodynamic drag force component of the accelerometermeasurements

$\begin{matrix}{{\text{low-pass}\left( {f_{sx}^{b}(t)} \right)} = {\frac{40^{4}}{\left( {s + 40} \right)^{4}}{{f_{sx}^{b}(t)}.}}} & (3.7)\end{matrix}$

The plots in FIG. 7 indicate that during ascent of the projectile 100sin γ>0 and during the descent of the projectile 100 the sin γ<0.

The tip of the unit vector v^(N)/∥v^(N)∥ is constrained to lie in theplane defined by the local Earth magnetic field vector. The projectile'svelocity vector resolved in the projectile's body frame is assumed to bein the same direction as the projectile's angular velocity vectorresolved in the projectile's body frame, so

$\begin{matrix}{{\frac{v^{N}}{v^{N}} \cdot \frac{m^{N}}{m^{N}}} = {{\frac{v^{b}}{v^{b}} \cdot \frac{m^{b}}{m^{b}}} \approx {\frac{m^{b}}{m^{b}} \cdot \frac{\omega^{b}}{\omega^{b}}} \approx \frac{m_{x}}{m^{b}}}} & (3.8)\end{matrix}$

where

m ^(b) =[m _(x) m _(y) m _(z)]^(T).  (3.9)

The dot product operator is independent of reference frame. Equations(3.5) and (3.8) are combined, so

$\begin{matrix}{{\begin{bmatrix}\frac{g^{NT}}{g^{N}} \\\frac{m^{NT}}{m^{N}}\end{bmatrix}\frac{v^{N}}{v^{N}}} = \begin{bmatrix}{{- \sin}\; \gamma} \\\frac{m_{x}}{m^{b}}\end{bmatrix}} & (3.10)\end{matrix}$

The tip of the unit vector v^(N)/∥v^(N)∥ must lie on the unit sphere andthe two planes defined by equation (3.10). The intersection of twoplanes is a line. In general, a line intersects a sphere at either twopoints, 1 point, or no points. In this case, the line defined byequation (3.10) intersects the unit sphere at two points. The two pointsindicate the possible directions of the projectile's velocity vectorresolved in the projectile's inertial frame. One of the two points isselected based on the projectile's launch direction (East or West)relative to the direction of the local Earth magnetic field vector. Thetwo possible directions of the unit vector v^(N)/∥v^(N)∥ are determined,first, by computing the equation of the line defined by equation (3.10)and, second, by computing the intersection points of the line with theunit sphere.

The equation of a line is written as

A(v _(po int) +v _(par))=y  (3.11)

where

$\begin{matrix}\begin{matrix}{A = \begin{bmatrix}\frac{g^{N}}{g^{N}} & \frac{m^{N}}{m^{N}}\end{bmatrix}^{T}} \\{y = \begin{bmatrix}{{- \sin}\; \gamma} \\\frac{m_{x}}{m^{b}}\end{bmatrix}} \\{\frac{v^{N}}{v^{N}} = {v_{point} + {v_{par}.}}}\end{matrix} & (3.12)\end{matrix}$

and where v_(po int)εR(A) the homogeneous solution and v_(par)εN(A)refers to the particular solution. The homogeneous solution v_(po int)is determined directly from equation (3.11)

v _(po int) =A ^(T)(AA ^(T))⁻¹ y.  (3.13)

The matrix AA^(T) is

$\begin{matrix}\begin{matrix}{{AA}^{T} = {\begin{bmatrix}\frac{g^{N}}{g^{N}} & \frac{m^{N}}{m^{N}}\end{bmatrix}^{T}\begin{bmatrix}\frac{g^{N}}{g^{N}} & \frac{m^{N}}{m^{N}}\end{bmatrix}}} \\{= {\begin{bmatrix}1 & \begin{matrix}\frac{g^{NT}}{g^{N}} & \frac{m^{N}}{m^{N}}\end{matrix} \\\begin{matrix}\frac{m^{NT}}{m^{N}} & \frac{g^{N}}{g^{N}}\end{matrix} & 1\end{bmatrix}.}}\end{matrix} & (3.14)\end{matrix}$

The determinant of AA^(T) is

$\begin{matrix}\begin{matrix}{{\det \left( {AA}^{T} \right)} = {1 - \begin{pmatrix}\frac{g^{NT}}{g^{N}} & \frac{m^{N}}{m^{N}}\end{pmatrix}^{2}}} \\{= {1 - {\left( {\frac{g^{N}}{g^{N}} \cdot \frac{m^{N}}{m^{N}}} \right)^{2}.}}}\end{matrix} & (3.15)\end{matrix}$

The angle α is defined as the angle between {right arrow over (g)} and{right arrow over (m)} resolved in the projectile's body frame, so

$\begin{matrix}\begin{matrix}{{\det \left( {AA}^{T} \right)} = {1 - {\cos^{2}\alpha}}} \\{= {\sin^{2}\alpha}} \\{= {{\frac{g^{N}}{g^{N}} \times \frac{m^{N}}{m^{N}}}}^{2}} \\{= {{\overset{->}{b}}^{2}.}}\end{matrix} & (3.16)\end{matrix}$

The inverse of AA^(T) is

$\begin{matrix}\begin{matrix}{\left( {AA}^{T} \right)^{- 1} = {\frac{1}{\det \left( {AA}^{T} \right)}{{cov}\left( {AA}^{T} \right)}}} \\{= {{\frac{1}{{\overset{->}{b}}^{2}}\begin{bmatrix}1 & {{- \frac{m^{NT}}{m^{N}}}\frac{g^{N}}{g^{N}}} \\{{- \frac{g^{NT}}{g^{N}}}\frac{m^{N}}{m^{N}}} & 1\end{bmatrix}}.}}\end{matrix} & (3.17)\end{matrix}$

Equations (3.12) and (3.17) are substituted into equation (3.13), so

$\begin{matrix}\begin{matrix}{v_{point} = {{{\frac{1}{{\overset{->}{b}}^{2}}\begin{bmatrix}\frac{g^{N}}{g^{N}} & \frac{m^{N}}{m^{N}}\end{bmatrix}}\begin{bmatrix}1 & {{- \frac{m^{NT}}{m^{N}}}\frac{g^{N}}{g^{N}}} \\{{- \frac{g^{NT}}{g^{N}}}\frac{m^{N}}{m^{N}}} & 1\end{bmatrix}}\begin{bmatrix}{{- \sin}\; \gamma} \\\frac{m_{x}}{m^{b}}\end{bmatrix}}} \\{= {{{\frac{1}{{\overset{->}{b}}^{2}}\begin{bmatrix}{\frac{g^{N}}{g^{N}} - {\frac{g^{NT}}{g^{N}}\frac{m^{N}}{m^{N}}\frac{m^{N}}{m^{N}}}} & {{{- \frac{m^{NT}}{m^{N}}}\frac{g^{N}}{g^{N}}\frac{g^{N}}{g^{N}}} + \frac{m^{N}}{m^{N}}}\end{bmatrix}}\begin{bmatrix}{{- \sin}\; \gamma} \\\frac{m_{x}}{m^{b}}\end{bmatrix}}.}}\end{matrix} & (3.18)\end{matrix}$

The particular solution v_(par) is determined by noting that thecross-product {right arrow over (b)}, defined in equation (3.16), isperpendicular to both unit vectors g^(N)/∥g^(N)∥ and m^(N)/∥m^(N)∥.

Therefore, {right arrow over (b)}εN(A) and the unit vector v^(N)/∥v^(N)∥are written as

$\begin{matrix}{{\frac{v^{N}}{v^{N}} = {v_{point} + {\xi \; b^{N}}}},} & (3.19)\end{matrix}$

where ξε

. The scalar ξ is determined where the line defined by equation (3.10)intersects the unit sphere

$\begin{matrix}\begin{matrix}{{\frac{v^{N}}{v^{N}} \cdot \frac{v^{N}}{v^{N}}} = 1} \\{= {\left( {v_{point} + {\xi \; b^{N}}} \right)^{T}\left( {v_{point} + {\xi \; b^{N}}} \right)}} \\{= {{v_{point}}^{2} + {\xi^{2}{b^{N}}^{2}}}} \\{{\xi^{2} = \frac{1 - {v_{point}}^{2}}{{b^{N}}^{2}}},}\end{matrix} & (3.20)\end{matrix}$

where v_(po int) ^(T)b^(N)=0. Equation (3.20) is substituted intoequation (3.19), then

$\begin{matrix}{\frac{v^{N}}{v^{N}} = {v_{point} \pm {\sqrt{1 - {v_{point}}^{2}}{\frac{b^{N}}{{b^{N}}^{2}}.}}}} & (3.21)\end{matrix}$

Since a number of assumptions and simplifications have been made toderive the magnitude and direction of the projectile's velocity vectorresolved in the projectile's NED frame verification is now demonstrated.The components of projectile's true velocity vector and estimatedvelocity vector resolved in the projectile's navigation frame areplotted to verify that the projectile's estimated velocity vectorcaptures the dynamics of the projectile's true velocity vector. FIG. 8shows the plots of the scalar components of the true velocity vector(V_(exact x), V_(exact y), V_(exact z)) and estimated velocity vector(V_(approx x), V_(approx y), V_(approx z)) resolved in the projectile'sNorth East Down frame for a 155 nm projectile, spin stabilized about itsminor axis with a spin rate of 100 Hz. A 4^(th)-order low-pass filterwas used to estimate the projectile's speed from the aerodynamic dragforce component of the accelerometer measurements, equation (3.7).

The projectile's velocity vector and angular velocity vector are assumedto be aligned when resolved in the projectile's body frame, so

$\begin{matrix}{v^{b} = {{v^{b}}{\frac{\omega^{b}}{\omega^{b}}.}}} & (3.22)\end{matrix}$

The direction of the projectile's angular velocity vector in theprojectile's body frame is determined as follows. Equation (2.41) isfiltered by a high-pass filter, and from equations (2.42) and (2.44)

$\begin{matrix}{{\text{high-pass}\left( f_{sx}^{b} \right)} \approx {{\frac{\rho \; S{\overset{->}{v}}^{2}r_{px}C_{L\; \alpha}}{2\; J_{z}}\left( {{\beta \; r_{y}} + {\alpha \; r_{z}}} \right)} + {2{{\omega_{x}\left( {{\omega_{y}r_{y}} + {\omega_{z}r_{z}}} \right)}.}}}} & (2.42)\end{matrix}$

The origin of the sensor measurement frame is assumed to be in the{circumflex over (x)}_(b)-ŷ_(b) plane, the accelerometer measurementaxis is assumed to be nominally aligned with the {circumflex over(x)}_(b)-axis, then ω_(x) dominates equation (2.42), and

$\begin{matrix}\begin{matrix}{r_{z} = 0} \\{{\text{high-pass}\left( f_{sx}^{b} \right)} \approx {2\omega_{x}\omega_{y}r_{y}}} \\{= {2{\omega_{x}\begin{bmatrix}0 & r_{y} & 0\end{bmatrix}}{\omega^{b}.}}}\end{matrix} & (3.23)\end{matrix}$

The local Earth magnetic field vector

{right arrow over ({dot over (m)}={right arrow over ({dot over (m)}|_(b) +{right arrow over (ω)}×{right arrow over (m)}  (3.24)

is differentiated.

The local Earth magnetic field vector is assumed to be constantthroughout the projectile's flight time, so

{right arrow over ({dot over (m)}={right arrow over (0)}  (3.25)

Equation (3.25) is substituted into equation (3.24), so

{right arrow over ({dot over (m)}| _(b) =−{right arrow over (ω)}×{rightarrow over (m)}={right arrow over (m)}×{right arrow over (ω)}  (3.26)

Equation (3.26) is resolved in the projectile's body frame, so

{dot over (m)}=m ^(bx)ω^(b)  (3.27)

Equations (3.23) and (3.27) are combined, so

$\begin{matrix}{\begin{bmatrix}\frac{\text{high-pass}\left( f_{sx}^{b} \right)}{2\omega_{x}} \\{\overset{.}{m}}^{b}\end{bmatrix} = {\begin{bmatrix}\begin{bmatrix}0 & r_{y} & 0\end{bmatrix} \\m^{b \times}\end{bmatrix}\mspace{14mu} {\omega^{b}.}}} & (3.28)\end{matrix}$

By defining

$\begin{matrix}{{A = \begin{bmatrix}\begin{bmatrix}0 & r_{y} & 0\end{bmatrix} \\m^{b \times}\end{bmatrix}}{y = \begin{bmatrix}\frac{\text{high-pass}\left( f_{sx}^{b} \right)}{2\omega_{x}} \\{\overset{.}{m}}^{b}\end{bmatrix}}} & (3.29)\end{matrix}$

and substituting equation (3.29) into equation (3.28), then

Aω ^(b) =y

A ^(T) Aω ^(b) =A ^(T) y

ω^(b)=(A ^(T) A)⁻¹ A ^(T) y  (3.30)

The matrix A^(T)A is

$\begin{matrix}\begin{matrix}{{A^{T}A} = {\begin{bmatrix}\begin{bmatrix}0 \\r_{y} \\0\end{bmatrix} & {- m^{b \times}}\end{bmatrix}\begin{bmatrix}\begin{bmatrix}0 & r_{y} & 0\end{bmatrix} \\m^{b \times}\end{bmatrix}}} \\{= {{\begin{bmatrix}0 \\r_{y} \\0\end{bmatrix}\begin{bmatrix}0 \\r_{y} \\0\end{bmatrix}}^{T} - {\left( m^{b \times} \right)^{2}.}}}\end{matrix} & (3.31)\end{matrix}$

The matrix A^(T)A is singular if {right arrow over (m)} resolved in theprojectile's body frame is in the {circumflex over (x)}_(b)−{circumflexover (z)}_(b). This 3D attitude occurs twice during the projectile'srotation about the {circumflex over (x)}_(b)-axis. Therefore, a smallconstant ε²I is added to A^(T)A to ensure the resulting matrix isnon-singular. The matrix A^(T)y is written as:

$\begin{matrix}\begin{matrix}{{A^{T}y} = {\begin{bmatrix}\begin{bmatrix}0 \\r_{y} \\0\end{bmatrix} & {- m^{b \times}}\end{bmatrix}\begin{bmatrix}\frac{\text{high-pass}\left( f_{sx}^{b} \right)}{2\omega_{x}} \\{\overset{.}{m}}^{b}\end{bmatrix}}} \\{= {{\begin{bmatrix}0 \\r_{y} \\0\end{bmatrix}\frac{\text{high-pass}\left( f_{sx}^{b} \right)}{2\omega_{x}}} + {{\overset{.}{m}}^{b \times}{m^{b}.}}}}\end{matrix} & (3.32)\end{matrix}$

Equations (3.31) and (3.32) are substituted into equation (3.30), then

$\begin{matrix}{\omega^{b} = {\left( {{\begin{bmatrix}0 \\r_{y} \\0\end{bmatrix}\begin{bmatrix}0 \\r_{y} \\0\end{bmatrix}}^{T} - \left( m^{b \times} \right)^{2} + {ɛ^{2}I}} \right)^{- 1}{\left( {{\begin{bmatrix}0 \\r_{y} \\0\end{bmatrix}\frac{\text{high-pass}\left( f_{sx}^{b} \right)}{2\omega_{x}}} + {{\overset{.}{m}}^{b \times}m^{b}}} \right).}}} & (3.33)\end{matrix}$

Thus, it has been shown how to determine the scalar components of thelocal Earth magnetic field vector and the projectile's velocity vectorin the projectile's NED frame and body frame. The projectile's attitudeand heading angle is computed from Ĉ_(bN) _(—) _(final) shown inequation (3.1).

FIG. 9 is a flow diagram of the vector matching algorithm 133. The inputto the low-pass filter 500 and the high-pass filter 501 is f_(sx), fromthe single axis accelerometer 120. The magnetic field measurement m^(b)is input to the algorithm 510. The magnetic field for the inertial framem^(N) provided or measured prior to launch of the projectile 100 isinput to the algorithm 512. The algorithm 514 is used to calculate thedirection of the velocity unit vector for the unit sphere and the twoplanes. The output of algorithm 514 is the velocity unit vector{circumflex over (v)}^(N)/∥{circumflex over (v)}^(N)∥ in the inertialframe which is input to algorithm 522. The output of the algorithm 516is the angular velocity vector ω^(b) in the body frame. The output fromalgorithm 518 is the direction of the velocity unit vector {circumflexover (v)}^(b)/∥{circumflex over (v)}^(b)∥ in the body frame, which isinput to the algorithm 520. The output of algorithm 520 times theinverse of the output of the algorithm 522 (see algorithm 524) is thedirection cosine matrix C_(bN) from which the projectile's attitude andheading angle are computed.

The fast spinning projectile attitude and heading angle reference system(AHRS) algorithm (also referred to herein as attitude estimator module130) developed above is now simulated for an exemplary embodiment of aprojectile 100. The fast spinning projectile AHRS algorithm performanceis evaluated in estimating the attitude and heading angle of theprojectile 100, to identify the error sources of the AHRS algorithm, andto quantify the effect of the errors sources on the performance of theAHRS algorithm.

The following discussion is based on a simulation of a fast spinningprojectile having the following characteristics:

$\begin{matrix}{{m_{b} = {45\mspace{14mu} {kg}}}\text{}{{diameter} = {0.155\mspace{14mu} m}}\text{}{{length} = {0.8\mspace{14mu} m}}{J = {\left\lbrack \begin{matrix}0.136 & 0 & 0 \\0 & 2.483 & 0 \\0 & 0 & 2.483\end{matrix} \right\rbrack \mspace{14mu} {kg}\; m^{2}}}} & (4.1) \\{{C_{D0} = 0.3}{C_{L\; \alpha} = {2.7/{rad}}}{C_{M\; \alpha} = {1/{rad}}}{C_{LP} = {C_{MQ} = {C_{NR} = {{- 0.1}\mspace{14mu} {\text{rad/s}.}}}}}} & (4.2)\end{matrix}$

The following discussion is based on the fast spinning projectile havingthe following trajectory characteristics:

ρ=1 kg/m³

γ(0)=36°

∥{right arrow over (v)}∥=620 m/s in {circumflex over (x)}_(b)-axisdirection

ω_(x)(0)=100 Hz.  (4.3)

Since a number of assumptions and simplifications were used to derivethe fast spinning projectile AHRS algorithm, the estimation error of thedirection cosine matrix estimation errors computed using the vectormatching algorithm 133 (shown in FIG. 9) are plotted. FIG. 10 shows theplots of the direction cosine matrix estimation error computed using thevector matching algorithm 133. The direction cosine matrix estimationerror was computed as follows

δC=∥I−(Ĉ _(bN) _(—) _(final))^(T) C _(bN)∥  (4.4)

FIG. 11 shows the estimation errors of the Euler Angles (φ-φ_(calc)),(θ-θ_(calc)), and (ψ-ψ_(calc)) and computed using the vector matchingalgorithm 133. The plot of (φ-φ_(calc)) is hidden by the plot of(ψ-ψ_(calc)) except where indicated in the region past 50 seconds. Asshown in FIG. 11, the maximum Euler angle errors (φ-φ_(calc)),(θ-θ_(calc)), and (ψ-ψ_(calc)) are less than 0.1 radians for theprojectile's entire flight.

The methods and techniques described here may be implemented in digitalelectronic circuitry, or with a processor 140 (for example, aspecial-purpose processor or a general-purpose processor such as acomputer, microprocessor, or microcontroller) firmware, software, or incombinations of them. In one implementation, the processor 140 comprisesprocessor support chips and/or system support chips such asapplication-specific integrated circuits (ASICs). Apparatus embodyingthese techniques may include appropriate input and output devices, aprogrammable processor, and a storage medium 131 tangibly embodyingprogram instructions for execution by the programmable processor. Aprocess embodying these techniques may be performed by a processor 140executing a program of instructions to perform desired functions byoperating on input data and generating appropriate output. Thetechniques may advantageously be implemented in one or more programsthat are executable on a programmable system including at least oneprogrammable processor 140 coupled to receive data and instructionsfrom, and to transmit data and instructions to, a data storage system,at least one input device, and at least one output device. Generally,the processor 140 receives instructions and data from a memory 135, suchas a read-only memory and/or a random access memory.

The attitude estimator module 130 (software 130) comprises a set ofprogram instructions embodied on the storage medium 131 from which atleast a portion of the program instructions are read by the processor140 for execution thereby. The program instructions, when executed bythe processor 140, carry out at least a portion of the functionalitydescribed here as being performed by the apparatus 10.

Storage devices suitable for tangibly embodying computer programinstructions and data include all forms of non-volatile memory,including by way of example semiconductor memory devices, such as EPROM,EEPROM, USB memory devices (i.e., thumb drives) and flash memorydevices; magnetic disks such as internal hard disks and removable disks;magneto-optical disks; and DVD disks. Any of the foregoing may besupplemented by, or incorporated in, specially-designedapplication-specific integrated circuits (ASICs).

FIG. 12 is a flow diagram of one embodiment of a method 1200 to measureand determine a three-dimensional attitude of a spinning projectileduring a flight of the spinning projectile in accordance with thepresent invention. The embodiment of method 1200 is described as beingimplemented using the apparatus 10 of FIG. 2 in a projectile 100 ofFIGS. 1 and 3. In such an embodiment, at least a portion of theprocessing of method 1200 is performed by the attitude estimator module130 executing on the processor 140 of the apparatus 10.

At block 1202, a magnetic unit vector in an inertial frame of theprojectile 100 at the projectile launch location 4 is provided prior tolaunch of the projectile 100. In one implementation of this embodiment,the magnetic unit vector in an inertial frame of the projectile 100 atthe projectile launch location 4 is stored in the memory 135 prior tolaunch.

At block 1204, a magnetic unit vector is determined in a body frame andin an inertial frame of the spinning projectile 100 during a flight ofthe spinning projectile 100. The magnetic unit vector is determined bymeasuring a body-axis magnetic vector m^(b) using the three-axismagnetometer 100 during the flight of the projectile 100 and byexecuting algorithms, such as algorithms in the attitude estimatormodule 130, at a processor 140 on board the projectile 100. Thealgorithms use the magnetic unit vector m^(N) of the inertial framestored in the memory 135.

At block 1206, a velocity unit vector is determined in the body frameF_(b) and in the inertial frame F_(N) of the spinning projectile 100during the flight of the spinning projectile 100. The velocity unitvector is determined in the body frame F_(b) and in the inertial frameF_(N) of the spinning projectile by measuring the specific force actingon the projectile using a single-axis accelerometer 120 having a senseaxis 121 aligned with a spin axis X_(p) of the spinning projectile 100and by determining a flight path angle γ as a function of time for thespinning projectile based on the measured specific force. The flightpath angle γ is determined as a function of time of the spinningprojectile 100 by applying a low-pass filter 129 to an accelerometeroutput to estimate drag on the spinning projectile 100. Determining thevelocity unit vector in the inertial frame F_(N) based on theapproximate flight path angle γ includes solving for the velocity unitvector as an intersection of a unit sphere 200 and two planes. A lineformed from the two intersecting planes intersects the unit sphere 200in two points. One of the two points is selected based on a direction oflaunch of the projectile (i.e., eastward or westward launching). Theparameters of the line intersecting the unit sphere are calculated. Thedirection of the velocity unit vector in the body frame F_(b) is alsodetermined based on the assumption that the velocity unit vector isparallel to the angular velocity unit vector in the body frame. Theangular velocity unit vector is determined by high-pass filtering thesingle-axis accelerometer measurements and by calculating the timederivative of the local Earth magnetic field vector in the inertialframe and body frame. Typically, low-cost rate gyroscopes are unable todirectly measure the angular velocity of a projectile spinning at ratesgreater than 100 Hz. In applications where the low-cost rate gyroscopesare able to measure the angular velocity of the spinning projectile, theangular velocity unit vector in the body frame is determined directlyfrom the angular velocity measurements.

At block 1208, the roll angle, the pitch angle, and the heading angle ofthe spinning projectile are calculated during the flight of the spinningprojectile, regardless of the spin rate of the projectile 100. The rollangle and the pitch angle of the spinning projectile comprise anattitude of the spinning projectile. The attitude and the heading angleof the spinning projectile are calculated by: generating a directioncosine matrix C by matching the magnetic unit vector and velocity unitvector in the body frame with the magnetic unit vector and velocity unitvector in the inertial frame; solving Wahba's problem for the directioncosine matrix C; and calculating the attitude and the heading angle fromthe direction cosine matrix. The correct solution to Wahba's problem isthe attitude and the heading angle of the spinning projectile 100.

In this manner, an accurate, reliable, and low-cost guidance,navigation, and control capability for projectiles that includes anattitude and heading angle reference system (apparatus 10 or 11) is usedto determine the roll angle and pitch angle (attitude), and headingangle of a spinning projectile during a flight of the spinningprojectile even for fast spinning projectiles, such as gun-launchedprojectiles with initial angular velocities greater than 100 Hz.

Although specific embodiments have been described herein, it will beappreciated by those of skill in the art that other algorithms andprotocols for a master-slave wireless network may be substituted for thespecific embodiments described. This application is intended to coverany adaptations and variations of the present invention. Therefore it ismanifestly intended that this invention be limited only by the claimsand the equivalents thereof.

1. A method to determine roll angle, pitch angle, and heading angle of aspinning projectile during a flight of the spinning projectile, themethod comprising: providing a magnetic unit vector in an inertial frameof the projectile at a projectile launch location prior to launch of theprojectile; determining a magnetic unit vector in a body frame and in aninertial frame of the spinning projectile during the flight of thespinning projectile; determining a velocity unit vector in the bodyframe and in the inertial frame of the spinning projectile during theflight of the spinning projectile; and calculating the roll angle, thepitch angle, and the heading angle of the spinning projectile during theflight of the spinning projectile, regardless of the spin rate of theprojectile, wherein the roll angle and the pitch angle of the spinningprojectile comprise an attitude of the spinning projectile.
 2. Themethod of claim 1, wherein determining the magnetic unit vector in thebody frame and in the inertial frame of the spinning projectilecomprises: measuring a body-axis magnetic vector using a three-axismagnetometer during the flight of the projectile; and executingalgorithms at a processor on board the projectile, wherein thealgorithms use the magnetic unit vector in the inertial frame at theprojectile launch location.
 3. The method of claim 1, whereindetermining the velocity unit vector in the body frame and in theinertial frame of the spinning projectile comprises: measuring aspecific force using a single-axis accelerometer having a sense axisaligned with a spin axis of the spinning projectile; and determining aflight path angle as a function of time for the spinning projectilebased on the measured specific force.
 4. The method of claim 3, furthercomprising: applying a low-pass filter to an output of the single-axisaccelerometer to estimate drag on the spinning projectile; anddetermining a magnitude of a velocity vector, wherein the determiningthe flight path angle is based on the determined magnitude of thevelocity vector.
 5. The method of claim 3, wherein the determining thevelocity unit vector in the inertial frame is based on the determinedflight path angle, the method further comprising: solving for thevelocity unit vector as an intersection of a unit sphere and two planes,wherein a line formed from the two intersecting planes intersects theunit sphere in two points; selecting one of the two points based on adirection of launch of the projectile; and calculating the parameters ofthe line intersecting the unit sphere.
 6. The method of claim 5, furthercomprising: assuming the velocity unit vector in the inertial frame isparallel to the angular velocity unit vector in the body frame; applyinga high-pass filter to an output of the single-axis accelerometer;calculating the time derivative of the local Earth magnetic field vectorin the inertial frame and the body frame; generating the angularvelocity unit vector in the body frame based on output from thehigh-pass filter and the time derivatives of the local Earth magneticfield vector; and determining a direction of the velocity unit vector inthe body frame based on the generated angular velocity unit vector inthe body frame.
 7. The method of claim 1, wherein calculating the rollangle, the pitch angle, and the heading angle of the spinning projectilecomprises: generating a direction cosine matrix by matching the magneticunit vector and the velocity unit vector in the body frame with themagnetic unit vector and the velocity unit vector in the inertial frame;solving Wahba's problem for the direction cosine matrix; and calculatingthe roll angle, the pitch angle, and the heading angle from thedirection cosine matrix, wherein the correct solution to Wahba's problemis the roll angle, the pitch angle, and the heading angle of thespinning projectile.
 8. The method of claim 1, further comprising:aligning a sense axis of a single-axis accelerometer with a spin axis ofthe spinning projectile; filtering output from the single-axisaccelerometer to determine drag acting on the spinning projectile;determining a magnitude of the velocity unit vector of the spinningprojectile; and determining an angular velocity unit vector of thespinning projectile.
 9. The method of claim 8, wherein filtering outputfrom the single-axis accelerometer includes filtering output from thesingle-axis accelerometer positioned on the spinning projectile spinningat a spin rate greater than 100 revolutions per second.
 10. The methodof claim 1, further comprising orienting a sense axis of a single-axisaccelerometer parallel to a spin axis of the projectile prior to launchof the projectile.
 11. The method of claim 1, wherein calculating theroll angle, the pitch angle, and the heading angle of the spinningprojectile includes calculating the roll angle, the pitch angle, and theheading angle of the projectile spinning more than 100 revolutions persecond.
 12. An apparatus to measure a three-dimensional attitude of aspinning projectile during a flight of the spinning projectile, theapparatus including: a three-axis magnetometer to measure the magneticfield of earth; a single-axis accelerometer having a sense axis parallelto a spin axis of the spinning projectile during the flight of thespinning projectile; and an attitude estimator module; at least oneprocessor to receive outputs from the three-axis magnetometer and thesingle-axis accelerometer and to execute at least one algorithm in theattitude estimator module to determine an attitude and heading angle ofthe spinning projectile during the flight of the spinning projectile,regardless of a spin rate of the projectile, based on outputs from thethree-axis magnetometer and the single-axis accelerometer, wherein theattitude and the heading angle comprise the three-dimensional attitude.13. The apparatus of claim 12, further comprising a Kalman filter toreceive output from the attitude estimator module, wherein the Kalmanfilter computes estimates of the attitude and the heading angle of thespinning projectile.
 14. The apparatus of claim 12, wherein the attitudeestimator receives from the three-axis magnetometer informationindicative of a local Earth magnetic field vector at the projectilelaunch site prior to launch, and wherein the attitude estimator moduleincludes the at least one algorithm to: use a low-pass filter onaccelerometer measurements to compute the drag acting on the projectile;estimate a velocity unit vector in the inertial frame; estimate avelocity unit vector in the body frame; generate two 3×3 unit vectormatrices; invert one 3×3 unit vector matrix; multiply one 3×3 unitvector matrix with the inverted 3×3 unit vector matrix to solve forWahba's problem; and use a singular value decomposition to generate adirection cosine matrix from the direction cosine matrix calculated tosolve Wahba's problem in order to enforce orthogonality constraints onthe direction cosine matrix calculated to solve Wahba's problem.
 15. Theapparatus of claim 12, further comprising a Kalman filtercommunicatively coupled to receive output from the attitude estimatormodule.
 16. A non-transitory computer readable medium embodying aprogram of instructions executable by a processor to perform a methodcomprising: determining a magnetic unit vector in a body frame and in aninertial frame of the spinning projectile during a flight of thespinning projectile; determining a velocity unit vector in the bodyframe and in the inertial frame of the spinning projectile during theflight of the spinning projectile; and calculating a roll angle, a pitchangle, and a heading angle of the spinning projectile during the flightof the spinning projectile, regardless of a spin rate of the projectile,wherein the roll angle, the pitch angle, and the heading angle of thespinning projectile comprise a three-dimensional attitude of thespinning projectile.
 17. The medium of claim 16, comprising the programof instructions executable by the processor to perform the method, whichfurther comprises: solving for the velocity unit vector in the inertialframe as an intersection of a unit sphere and two planes, wherein a lineformed from the two intersecting planes intersects the unit sphere intwo points; selecting one of the two points based on a direction oflaunch of the projectile; and calculating parameters of the lineintersecting the unit sphere.
 18. The medium of claim 17, comprising theprogram of instructions executable by the processor to perform themethod, which further comprises: applying a high-pass filter to anoutput of the single-axis accelerometer; calculating the time derivativeof the local Earth magnetic field vector in the inertial frame and thebody frame generating the angular velocity unit vector in the body framebased on output from the high-pass filter and the time derivatives ofthe local Earth magnetic field vector; and determining a direction ofthe velocity unit vector in the body frame based on the generatedangular velocity unit vector in the body frame, wherein the velocityunit vector in the body frame is assumed to be parallel to the angularunit velocity in the body frame.
 19. The medium of claim 17, comprisingthe program of instructions executable by the processor to perform themethod, which further comprises: generating a direction cosine matrix bymatching the magnetic unit vector and the velocity unit vector in thebody frame with the magnetic unit vector and the velocity unit vector inthe inertial frame; solving Wahba's problem for the direction cosinematrix; and calculating the roll angle, the pitch angle, and the headingangle from the direction cosine matrix, wherein the correct solution toWahba's problem is the roll angle, the pitch angle, and the headingangle of the spinning projectile.
 20. The medium of claim 16, comprisingthe program of instructions executable by the processor to perform themethod, which further comprises: using a low-pass filter onaccelerometer measurements to compute the drag acting on the projectile;estimating a velocity unit vector in the inertial frame; estimating avelocity unit vector in the body frame; generating two 3×3 unit vectormatrices; inverting one 3×3 unit vector matrix; multiplying one 3×3 unitvector matrix with the inverted 3×3 unit vector matrix to solve forWahba's problem; and using a singular value decomposition to generate adirection cosine matrix from the direction cosine matrix calculated tosolve Wahba's problem in order to enforce orthogonality constraints onthe direction cosine matrix calculated to solve Wahba's problem.